// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GeneralIKTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Kinematics
{
    using System;
    using System.Collections.Generic;

    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;

    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using ShoNS.Array;

    /// <summary>
    /// A class that tests the general inverse kinematics.
    /// </summary>
    [TestClass]
    [DeploymentItem(@"ShoArray.dll")]
    [DeploymentItem(@"MatrixInterf.dll")]
    [DeploymentItem(@"ArrayT.dll")]
    [DeploymentItem(@"libiomp5md.dll")]
    [DeploymentItem(@"mkl.dll")]
    [DeploymentItem(@"msvcp110.dll")]
    [DeploymentItem(@"msvcr110.dll")]
    public class GeneralIKTests
    {
        /// <summary>
        /// Tests the inverse kinematics for the 7-DOF Kuka arm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void InverseKinematicsTestKuka7DOF1()
        {
            // create an kinematic chain for a 7-DOF Kuka arm
            KinematicChain kc = KinematicChainTests.CreateA7DOFKinematicChain();

            for (int i = 0; i < 10; ++i)
            {
                // Generate one joint configuration sample
                DoubleArray jointSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, 100, i + 1);

                // calculate end effector pose using forward kinematics
                Matrix4[] kinematics = kc.GetJointTransforms(jointSample);
                Matrix4[] kinematicsOriginal = kinematics;

                int numSolutions = 0;

                // generate different random seed for IK
                for (int j = 0; j < 10; ++j)
                {
                    DoubleArray seed = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, 100, j + 10);
                    bool maxIterationExceeded;
                    bool jointsWithinRange;
                    DoubleArray solution = InverseKinematicsNumerical.SolveIKNumerical(
                        kc: kc,
                        endEffectorPose: kinematics[7],
                        seed: seed,
                        maxNumIterationsExceeded: out maxIterationExceeded,
                        jointsWithinRange: out jointsWithinRange,
                        translationalTolerance: 0.001,
                        rotationalTolerance: 0.001,
                        maxNumIterations: 100);
                    if (true == maxIterationExceeded || false == jointsWithinRange)
                    {
                        continue;
                    }

                    // one more solution
                    ++numSolutions;

                    // compute the end effector pose using forward kinematics
                    Matrix4[] kinematicsValidate = kc.GetJointTransforms(solution);

                    // record test related information
                    string msg_joint = string.Format("failed solution for original joint sample [{0}, {1}, {2}, {3}, {4}, {5}, {6}]\n", jointSample[0], jointSample[1], jointSample[2], jointSample[3], jointSample[4], jointSample[5], jointSample[6]);
                    string msg_seed = string.Format("failed seed[{0}, {1}, {2}, {3}, {4}, {5}, {6}]\n", seed[0], seed[1], seed[2], seed[3], seed[4], seed[5], seed[6]);
                    string msg_solution_index = string.Format("failed solution index: {0}\n", j);
                    string msg = msg_joint + msg_seed + msg_solution_index;

                    PoseDiff diff = new PoseDiff(kinematicsOriginal[6], kinematicsValidate[6]);
                    Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff), 0, 1e-3, msg);
                }

                // we should make sure there are solutions found
                Assert.AreNotEqual(numSolutions, 0, "No solutions found");
            }
        }

        /// <summary>
        /// Tests the inverse kinematics on a 7-DOF Kuka arm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void InverseKinematicsTestKuka7DOF2()
        {
            // create a kinematic chain for a 7-DOF Kuka arm
            KinematicChain kc = KinematicChainTests.CreateA7DOFKinematicChain();

            for (int i = 0; i < 10; ++i)
            {
                // generate one joint sample
                DoubleArray jointSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, 100, i + 1);

                // calculate end effector pose using forward kinematics
                Matrix4[] kinematics = kc.GetJointTransforms(jointSample);
                Matrix4[] kinematicsOriginal = kinematics;

                List<DoubleArray> solutions = InverseKinematicsNumerical.SolveIKNumerical(kc, kinematics[7], 10);

                // validate solutions
                for (int j = 0; j < solutions.Count; ++j)
                {
                    DoubleArray solution = solutions[j];

                    // compute the end effector pose using forward kinematics
                    Matrix4[] kinematicsValidate = kc.GetJointTransforms(solution);

                    // record test related information
                    string msg_joint = string.Format("failed solution for original joint sample [{0}, {1}, {2}, {3}, {4}, {5}, {6}]\n", jointSample[0], jointSample[1], jointSample[2], jointSample[3], jointSample[4], jointSample[5], jointSample[6]);
                    string msg_solution_index = string.Format("failed solution index: {0}\n", j);
                    string msg = msg_joint + msg_solution_index;

                    PoseDiff diff = new PoseDiff(kinematicsOriginal[6], kinematicsValidate[6]);
                    Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff), 0, 1e-3, msg);
                }
            }
        }
    }
}
